#include <rclcpp/rclcpp.hpp>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/msg/pose.hpp>
#include <geometry_msgs/msg/point.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer.h>
// 暂时注释掉缺少的头文件
#include <sensor_msgs/msg/joint_state.hpp>
#include "sensor_msgs/msg/joint_state.hpp" 
#include <moveit_servo/servo_parameters.h>
#include <moveit_servo/servo.h>

#include <std_msgs/msg/float64.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <eigen3/Eigen/Dense>
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>

#include <chrono>

using namespace std::chrono_literals;
using namespace Eigen;  // 显式声明Eigen命名空间

static const rclcpp::Logger LOGGER = rclcpp::get_logger("visual_control_node");

class VisualControl : public rclcpp::Node {
public:
    VisualControl() : Node("visual_control_node")
    {
        // 初始化参数
        position_reference_ = Eigen::Vector3d(0.0, 0.0, 0.0);     // 参考位置，设为零

        velocity_ = Eigen::Vector3d::Zero();                      // 初始速度为零
        dt_ = 0.01; 
        current_position_.x() =0;
        current_position_.y() =0;
        current_position_.z() =0;
        current_visual_ = Eigen::Vector3d::Zero();    
        result_data_ = geometry_msgs::msg::Point(); // 初始化视觉结果
        // 订阅视觉处理结果
        // result_sub_ = this->create_subscription<geometry_msgs::msg::Point>(
        //     "/result3_topic", 10,                    // 订阅视觉结果话题，队列大小为10
        //     [this](const geometry_msgs::msg::Point::SharedPtr msg) {
        //         result_data_ = *msg;                 // 存储接收到的视觉处理结果数据
        //     });

        
        // 创建力传感器数据订阅者
        visual_sub_ = create_subscription<geometry_msgs::msg::TwistStamped>(
            "/result3_topic", 10, std::bind(&VisualControl::visual_callback, this, std::placeholders::_1));

        // 初始化TF监听器
        tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
        tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
        // 创建发布者，发布位置指令到moveit_servo
        // 设置发布者
        twist_cmd_pub_ = create_publisher<geometry_msgs::msg::TwistStamped>("visual_control_node/delta_twist_cmds", 10);

    
    }
    // 初始化函数，在对象构造完成后调用
    void initialize()
    {
        setupPlanningSceneMonitor();
        setupServo();

        // 获取机械臂的当前位姿作为参考位置
        // getCurrentPositionAsReference();

        // 创建定时器，25Hz运行控制循环（40ms）
        timer_ = create_wall_timer(std::chrono::milliseconds(40), std::bind(&VisualControl::control_loop, this));

        RCLCPP_INFO(this->get_logger(), "visual伺服节点已初始化");
    }


private:
    // 成员变量
    rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr visual_sub_;
    rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr tf_pub_;  // 结果数据发布器
    rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twist_cmd_pub_;
    rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pose_pub_;
    rclcpp::TimerBase::SharedPtr timer_; // 定时器，用于定期执行控制循环

    std::shared_ptr<planning_scene_monitor::PlanningSceneMonitor> planning_scene_monitor_;
    std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
    std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
    std::unique_ptr<moveit_servo::Servo> servo_;

    geometry_msgs::msg::Point result_data_;     // 存储视觉处理结果

    Eigen::Vector3d current_visual_;          // 当前
    Eigen::Vector3d current_position_;
    Eigen::Vector3d velocity_;               // 速度
    Eigen::Vector3d position_reference_;     // 参考位置
    double dt_;       
    
    
    void visual_callback(const geometry_msgs::msg::TwistStamped::SharedPtr msg)
    {
        (void)msg;
        // 从TwistStamped消息中提取当前力
        current_visual_.x() =0.05;
        current_visual_.y() = 0;
        current_visual_.z() = 0;
    }
    void control_loop()
    {
        // 更新速度
        velocity_.x() +=  0.01 ;
        if (velocity_.x() > 0.2 ){
            velocity_.x() = 0.2 ;// 限制速度
        }
        // 计算位置增量
        Eigen::Vector3d delta_position = velocity_ ;//* dt_
        // 计算新的位置
        Eigen::Vector3d new_position = current_position_ - delta_position;

        auto msg = std::make_unique<geometry_msgs::msg::TwistStamped>();
        msg->header.stamp = this->now();
        msg->header.frame_id = "base_link";
        
        msg->twist.linear.x = velocity_.x();
        msg->twist.linear.y = velocity_.y();
        msg->twist.linear.z = velocity_.z();
        msg->twist.angular.x = 0.0;
        msg->twist.angular.y = 0.0;
        msg->twist.angular.z = 0.0;

        twist_cmd_pub_->publish(std::move(msg));

        RCLCPP_INFO(this->get_logger(), "Current velocity_x: %.3f", velocity_.x());
        // RCLCPP_INFO(this->get_logger(), "New position: x=%.3f, y=%.3f, z=%.3f",
        //             new_position.x(), current_position_.y(), current_position_.z());
    }

    // 获取机械臂当前的位姿并设置为参考位置
    // void getCurrentPositionAsReference()
    // {
    //     geometry_msgs::msg::TransformStamped transform;
    //     try {
    //         transform = tf_buffer_->lookupTransform("base_link", "end", tf2::TimePointZero);
    //         position_reference_.x() = transform.transform.translation.x;
    //         position_reference_.y() = transform.transform.translation.y;
    //         position_reference_.z() = transform.transform.translation.z;
    //         RCLCPP_INFO(this->get_logger(), "设置参考位置为: x=%.3f, y=%.3f, z=%.3f",
    //                     position_reference_.x(), position_reference_.y(), position_reference_.z());
    //     } catch (tf2::TransformException &ex) {
    //         RCLCPP_WARN(this->get_logger(), "无法获取TF变换: %s", ex.what());
    //     }
    // }

    void setupPlanningSceneMonitor()
    {
        planning_scene_monitor_ = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>(
            shared_from_this(), "robot_description", tf_buffer_, "planning_scene_monitor");

        if (planning_scene_monitor_->getPlanningScene()) {
            planning_scene_monitor_->startStateMonitor("/joint_states");
            planning_scene_monitor_->setPlanningScenePublishingFrequency(25);
            planning_scene_monitor_->startPublishingPlanningScene(
                planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE,
                "/moveit_servo/publish_planning_scene");
            planning_scene_monitor_->startSceneMonitor();
            planning_scene_monitor_->providePlanningSceneService();
        } else {
            RCLCPP_ERROR(LOGGER, "规划场景配置失败");
            throw std::runtime_error("Planning scene setup failed");
        }
    }

    void setupServo()
    {
        auto servo_parameters = moveit_servo::ServoParameters::makeServoParameters(shared_from_this());
        if (!servo_parameters) {
            RCLCPP_FATAL(LOGGER, "无法加载伺服参数");
            throw std::runtime_error("Failed to load servo parameters");
        }

        servo_ = std::make_unique<moveit_servo::Servo>(shared_from_this(), servo_parameters, planning_scene_monitor_);
        servo_->start();
    }// 时间步长tor3d acceleration =   
}; 
int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<VisualControl>();
    node->initialize();  // 在对象构造完成后调用初始化
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}